/*****************************************************************************/
/*                                                                           */
/*    Domino Operation System System Specific Module                         */
/*                                                                           */
/*    Copyright (C) 2010 Laszlo Arvai                                        */
/*                                                                           */
/*    ------------------------------------------------------------------     */
/*    netICMP - Internet Control Message Protocol implementation             */
/*****************************************************************************/

/*****************************************************************************/
/* Includes                                                                  */
/*****************************************************************************/
#include <netPacketBuffer.h>
#include <netICMP.h>
#include <netMAC.h>

/*****************************************************************************/
/* Constants                                                                 */
/*****************************************************************************/
#define netICMP_ECHO_REQUEST  0x08
#define netICMP_ECHO_REPLY    0x00

// header field positions
#define netICMP_HFP_TYPE			0
#define netICMP_HFP_CODE			1
#define netICMP_HFP_CHECKSUM	2
#define netICMP_HFP_ID				4
#define netICMP_HFP_SEQUENCE	6

/*****************************************************************************/
/* Functions                                                                 */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
//! Processes ICMP packet
//! \param IP header information of ICMP packet
void netICMPProcess( netIPHeaderInfo* header_info)
{
  dosByte packet_chunk_index;
  dosWord pos;
  dosByte type;
  dosByte code;
  dosDWord word_sum;
  dosWord checksum;

  // sanity check
  if( header_info->Protocol != netIPDT_ICMP )
  {
    netDeleteRXPacket();
    return;
  }
  
  // check ICMP header
  pos = netGetRXPacketPos();
  
  netReadByteRXPacket( &type );
  netReadByteRXPacket( &code );
  
  if( type != netICMP_ECHO_REQUEST || code != 0 )
  {
    netDeleteRXPacket();
    return;
  }
  
  // check CRC
  word_sum = 0;
  netSetRXPacketPos( pos );
  netUpdateRXPacketChecksum( header_info->PacketLength - header_info->HeaderLength, &word_sum );
  checksum = netICMPCalculateChecksum( word_sum );
  
  if( checksum != 0 )
  {
    netDeleteRXPacket();
    return;
  }

  // send response packet
  packet_chunk_index = netUnlinkRXPacket();
  
  // prepare this packet as a new TX packet
  netOpenTXPacket(packet_chunk_index);
  
  // create IP header
  netIPCreateHeader( header_info );

  // store ICMP header position
  pos = netGetTXPacketPos();

  // change to reply packet
  netWriteByteTXPacket( 0 );
          
  // update CRC
  netSetTXPacketPos( pos + netICMP_HFP_CHECKSUM );
  netWriteByteTXPacket( 0 );
  netWriteByteTXPacket( 0 );
  
  word_sum = 0;
  netSetTXPacketPos( pos );
  netUpdateTXPacketChecksum( header_info->PacketLength - header_info->HeaderLength, &word_sum );
  checksum = netICMPCalculateChecksum( word_sum );

  // store checksum
  netSetTXPacketPos( pos + netICMP_HFP_CHECKSUM );
  netWriteByteTXPacket( HIGH(checksum) );
  netWriteByteTXPacket( LOW(checksum) );
                                
  // transmit packet
  netSendTXPacket();
}

///////////////////////////////////////////////////////////////////////////////
//! Calculates ICMP checksum from sum of words
//! \param Sum of words
//! \return ICMP checksum
dosWord netICMPCalculateChecksum( dosDWord in_checksum )
{
  // now calculate the sum over the bytes in the sum
  // until the result is only 16bit long
  while( (in_checksum>>16) != 0 )
  {
    in_checksum = (in_checksum & 0xFFFF) + (in_checksum >> 16);
  }
  
  // one's complement
  return ~((dosWord)in_checksum);
}